clc, clear all;
close all;
%% 2、读取python处理后得到的z
% z_data = load('z_data_MR_132.txt'); % 132*4
% z_data = load('z_data_MR.txt'); % N*4 !用load打包成exe会把zdata一起打包，z_data写死
z_data = readmatrix('z_data_MR.txt');
% 设置PD的真实坐标PD_pos
% M = readmatrix('D:/可见光定位与通信项目/PPM/position/Newton_r/LED_PD_position.csv');
M = readmatrix('LED_PD_position.csv');
LED_pos = M(1:4, 2:4);
PD_pos = M(6:end, 2:4);
% stem3(PD_pos(:,1), PD_pos(:,2), PD_pos(:,3))
%% 3、利用二元牛顿迭代法 计算全部r和Psi
Pm = 0.01 * LED_pos;   % LED的坐标
num = 1; % r的组数
PDsize = size(PD_pos);
index_record = [];
% 报告用生成随机序列
index_random = randperm(132);
for iii = 1:65
        kk = index_random(iii);
        kkk = index_random(133-iii);
        index_record = [index_record, kk, kkk];
% for kk = 1:floor((PDsize(1)-2)/2) %两两分组
%         kkk = floor((PDsize(1)-2))-kk+1;
        % 读取real_data.mat中保存的数据
%         real_data = load('real_data.mat');
        k1 = kk;
        k2 = kkk;   
        xR_1 = 0.01 * PD_pos(k1, :);      % 第k个数据，接收器PD真实坐标
        xR_1(3) = 0.085;
        xR_2 = 0.01 * PD_pos(k2, :);
        xR_2(3) = 0.085;
        z_real_1 = z_data(k1, :);
        z_real_2 = z_data(k2, :);
        r0 = 2;
        Psi0 = -2;%迭代初始值        
        iterations = 100; % 迭代次数
        error = 1e-6;     % 允许的迭代精度        
        r_estimate = zeros(1,4);
        Psi_estimate = ones(1,4);
        for j = 1:4
            for i = 1:iterations
                f = z_r_Psi(r0, Psi0, xR_1, Pm(j,:)) - z_real_1(j);
                g = z_r_Psi(r0, Psi0, xR_2, Pm(j,:)) - z_real_2(j);                
                fx = z_pd_r(r0, Psi0, xR_1, Pm(j,:));
                fy = z_pd_Psi(r0, xR_1, Pm(j,:));
                gx = z_pd_r(r0, Psi0, xR_2, Pm(j,:));
                gy = z_pd_Psi(r0, xR_2, Pm(j,:));            
                r1 = r0 + (-1*f*gy + fy*g)/(fx*gy - fy*gx);
                Psi1 = Psi0 + (-1*fx*g + f*gx)/(fx*gy - fy*gx);           
                if (abs(r1-r0) < error) & (abs(Psi1-Psi0) < error)
                    r_estimate(j) = r1;
                    Psi_estimate(j) = Psi1;
                    break
                else
                    r0 = r1;
                    Psi0 = Psi1;
                end
            end
            r_tuple(num,j) = r1;
            Psi_tuple(num, j) = Psi1;
        end
        num = num + 1;    
end
%% 4、筛选数据，去掉NaN所在行，每列排序后去掉一些最大最小值，获得标定结果r和Psi
r_Psi = [r_tuple, Psi_tuple];
Shape = size(r_Psi);
i = 1;
num_nonan =1;
while i< Shape(1)
    for j = 1:1:8
        if isnan(r_Psi(i,j))
%             i = i +1;
            break
        elseif(j==8)
            r_Psi_2(num_nonan,:) = r_Psi(i,:);
            num_nonan =num_nonan+1;
        end
    end
    i = i+1;
end
r_Psi_new = r_Psi_2;
for i = 1:8
    r_Psi_sorted = sortrows(r_Psi_new, i);
    r_Psi_delete = r_Psi_sorted(2:length(r_Psi_sorted)-4 ,:);
    r_Psi_new = r_Psi_delete;
end
r_Psi_mean = mean(r_Psi_new, 1);
% save('r_Psi_mean.mat', 'r_Psi_mean');
% 保存迭代结果，给python读取，第一行r（1*4），第二行Psi（1*4）
final_result = [r_Psi_mean(1:4); r_Psi_mean(5:8);];
writematrix(final_result, 'r_Psi_result.csv')
%% 6、相关函数 ----------------------------------------------------------------
function derivation = z_pd_r(r, Psi, xR, Pm)
    % z关于r的一阶导数 
    Vm = [0 0 -1];          % LED方向
    u_R = [0 0 1];          % PD方向(固定为竖直向上，五维问题降成三维)
    em = xR-Pm;             % LED发射方向向量，由LED指向PD

    derivation = Psi * (em*u_R') * ((em*Vm')^r) * ((norm(em))^(-r-3)) * (1+(1+r)*log(em*Vm')-(1+r)*log(norm(em)));
end
function derivation = z_pd_Psi(r, xR, Pm)
    % z关于Psi_R的一阶导数 
    Vm = [0 0 -1];          % LED方向
    u_R = [0 0 1];          % PD方向(固定为竖直向上，五维问题降成三维)
    R = norm(xR-Pm);        % gm的分母, 真实位置与LED的直线距离
    derivation = 1*((r+1)*(Vm*(xR-Pm)').^r)*(xR-Pm)/R^(r+3)*u_R';
end
function Z = z_r_Psi(r, Psi, xR, Pm)
    % z是关于r和Psi的函数 
    u_R = [0 0 1];          % PD方向(固定为竖直向上，五维问题降成三维)
    Vm = [0 0 -1];          % LED方向
    R = norm(xR-Pm);        % gm的分母, 真实位置与LED的直线距离
    G = Psi*((r+1)*(Vm*(xR-Pm)').^r)*(xR-Pm)/R^(r+3); % 公式(5)和(6)之间的g_m(x_R)
    Z = G*u_R';              % 公式(6)
end
% -------------------------------------------------------------------------